# -*- coding: utf-8 -*-
'''
Created on 27.06.2019

@author: yu03
'''

import cv2
import matplotlib.pyplot as plt
from scipy import signal
from FFT_Interpolation import FFT_interpolation_boxcar
import numpy as np

Lamda = 633e-9
pix_size = 5.3e-6
V_x, V_y, V_z = 0, 0, 0

hor_angle_centers = []
ver_angle_centers = []
hor_phase_centers = []
ver_phase_centers = []

for j in range(11):
    pattern_path = r'C:\Users\yu03\Desktop\2019-06-26_18-39-10\Modulating(24cm)\X\test_%i.bmp'%j
#     pattern_path = r'C:\Users\yu03\Desktop\2019-06-26_18-39-10\Repeatability(24cm)\test_%i.bmp'%j
    img = cv2.imread(pattern_path, 0)
    # size = img.shape
    # print(size)
    # print(img[512][640])
    # cv2.imshow('image',img)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()
    
    hor_center = img[438][300:800]
    ver_center = img[:,577][250:650]
        
    hor_center = hor_center / signal.gaussian(len(hor_center), std=len(hor_center)/3)
    ver_center = ver_center / signal.gaussian(len(ver_center), std=len(ver_center)/2.5)
    
    hor_center = hor_center - np.average(hor_center)
    ver_center = ver_center - np.average(ver_center)
    
    hor_freq, hor_phase = FFT_interpolation_boxcar(hor_center, pix_size)[0:2]
    ver_freq, ver_phase = FFT_interpolation_boxcar(ver_center, pix_size)[0:2]
    
    hor_angle = V_x - Lamda * hor_freq / 2
    ver_angle = V_y - Lamda * ver_freq / 2
    
    
    hor_phase = hor_phase / 2 / np.pi * 360 ### in degree
    if hor_phase < 0:
        hor_phase += 360
        
    ver_phase = ver_phase / 2 / np.pi * 360 ### in degree
    if ver_phase < 0:
        ver_phase += 360
    
    hor_angle_centers.append(hor_angle)
    ver_angle_centers.append(ver_angle)
    hor_phase_centers.append(hor_phase)
    ver_phase_centers.append(ver_phase)
    
#     plt.figure(1)
#     plt.subplot(2,1,1)
#     plt.plot(hor_center)
#     plt.subplot(2,1,2)
#     plt.plot(ver_center)
#     plt.show()

plt.figure(2)
plt.subplot(2,2,1)
plt.plot(hor_angle_centers, color='blue', marker='o')
plt.title("Horizontal Tilting")
plt.ylabel("Horizontal Tilting (rad)")
plt.xlabel("Samples")
plt.grid(which='major', axis='both')
 
plt.subplot(2,2,2)
plt.plot(ver_angle_centers, color='blue', marker='o')
plt.title("Vertical Tilting")
plt.ylabel("Vertical Tilting (rad)")
plt.xlabel("Samples")
plt.grid(which='major', axis='both')

plt.subplot(2,2,3)
plt.plot(hor_phase_centers, color='blue', marker='o')
plt.title("Horizontal Phase")
plt.ylabel("Horizontal Phase (degree)")
plt.xlabel("Samples")
plt.grid(which='major', axis='both')
 
plt.subplot(2,2,4)
plt.plot(ver_phase_centers, color='blue', marker='o')
plt.title("Vertical Phase")
plt.ylabel("Vertical Phase (degree)")
plt.xlabel("Samples")
plt.grid(which='major', axis='both')
 
plt.tight_layout()
plt.show()


